// -*- coding: utf-8 -*-
#include "mission.h"
#include <locale>
#include <codecvt>

Waypoint::Waypoint(ActionType type, double x_val, double y_val, double z_val,
                   double yaw_val, double duration_val, double speed_val,
                   const std::string &name, const std::string &topic, double radius_val)
    : actionType(type), x(x_val), y(y_val), z(z_val), yaw(yaw_val),
      duration(duration_val), speed(speed_val), stepName(name), topicName(topic), radius(radius_val) {}

WaypointManager::WaypointManager() : initialized(false), actionInProgress(false), hasTopicControl(false), nh_ptr(nullptr)
{
}

WaypointManager::~WaypointManager()
{
    resetTopicControl();
}

void WaypointManager::setNodeHandle(ros::NodeHandle* nh) 
{
    nh_ptr = nh;
}

void WaypointManager::topicControlCallback(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
    topicControlMsg = *msg;
    hasTopicControl = true;
    ROS_INFO_THROTTLE(2.0, "Received topic control message from %s: [%.2f, %.2f, %.2f]", 
                     currentTopicName.c_str(),
                     msg->pose.position.x, msg->pose.position.y, msg->pose.position.z);
}

void WaypointManager::resetTopicControl()
{
    hasTopicControl = false;
    
    if (topicControlSub)
    {
        topicControlSub.shutdown();
        ROS_INFO_THROTTLE(1.0, "Topic control subscriber shutdown for: %s", currentTopicName.c_str());
    }
    
    topicControlMsg = geometry_msgs::PoseStamped();
    currentTopicName.clear();
    
    velocity_reset_flag = true;
    
    ROS_INFO_THROTTLE(1.0,  "Topic control state reset - all messages cleared and velocity control reset");
}

void WaypointManager::setupTopicSubscription(ros::NodeHandle &nh, const std::string &topicName)
{
    if (currentTopicName == topicName && topicControlSub)
    {
        ROS_INFO_THROTTLE(1.0,  "Already subscribed to topic: %s", topicName.c_str());
        return;
    }

    resetTopicControl();
    
    velocity_reset_flag = true;
    ROS_WARN("Topic switching from '%s' to '%s' - forcing velocity control reset", 
             currentTopicName.c_str(), topicName.c_str());

    currentTopicName = topicName;
    topicControlSub = nh.subscribe<geometry_msgs::PoseStamped>(
        topicName, 10, &WaypointManager::topicControlCallback, this);
    
    ROS_INFO_THROTTLE(1.0,  "Subscribed to new topic: %s", topicName.c_str());
}

bool WaypointManager::hasNextWaypoint()
{
    return initialized && (currentWaypoint != waypoints.end());
}

Waypoint WaypointManager::getCurrentWaypoint()
{
    if (currentWaypoint != waypoints.end())
    {
        return *currentWaypoint;
    }
    return Waypoint(MOVE_TO_POINT, 0, 0, 0, 0, 0, 0, "invalid");
}

void WaypointManager::moveToNextWaypoint()
{
    if (currentWaypoint != waypoints.end())
    {
        Waypoint current = getCurrentWaypoint();
        if (current.actionType == TOPIC_CONTROL)
        {
            resetTopicControl();
            ROS_INFO_THROTTLE(1.0,  "Topic control ended, moving to next waypoint");
        }
        
        ++currentWaypoint;
        actionInProgress = false;
        
        velocity_reset_flag = true;
        ROS_INFO_THROTTLE(1.0,  "Moving to next waypoint - velocity control will be reset");
    }
}

void WaypointManager::startCurrentAction()
{
    missionStartTime = ros::Time::now();
    actionStartTime = ros::Time::now();
    actionInProgress = true;

    Waypoint current = getCurrentWaypoint();
    if (current.actionType == ROTATE)
    {
        initialYaw = tf::getYaw(current_pose.pose.orientation);
        resetRotationPosition();
    }
    else if (current.actionType == HOVER || current.actionType == WAIT)
    {
        resetHoldPosition();
    }
    else if (current.actionType == TOPIC_CONTROL)
    {
        if (nh_ptr)
        {
            velocity_reset_flag = true;
            resetHoldPosition();
            
            ROS_WARN("Starting topic control - forcing velocity and hold reset to prevent teleportation");
            
            setupTopicSubscription(*nh_ptr, current.topicName);
            ROS_INFO_THROTTLE(1.0,  "Starting topic control for: %s (duration: %.1fs)", 
                     current.topicName.c_str(), current.duration);
        }
        else
        {
            ROS_ERROR("NodeHandle not set! Cannot subscribe to topic control.");
        }
    }
}

void WaypointManager::resetHoldPosition()
{
    reset_hold_state();
}

void WaypointManager::resetRotationPosition()
{
    reset_rotation_state();
}

void WaypointManager::addMovePoint(double x, double y, double z, const std::string &name, double speed)
{
    waypoints.emplace_back(MOVE_TO_POINT, x, y, z, speed, 0, 0.5, name);
}

void WaypointManager::addRotation(double yaw, const std::string &name)
{
    waypoints.emplace_back(ROTATE, 0, 0, 0, yaw, 0, 0, name);
}

void WaypointManager::addTopicControl(const std::string &topicName, double duration, const std::string &name)
{
    waypoints.emplace_back(TOPIC_CONTROL, 0, 0, 0, 0, duration, 0, name, topicName);
}

void WaypointManager::addSuperControl(double x, double y, double z, const std::string &topicName, double duration, const std::string &name)
{
    waypoints.emplace_back(SUPER_CONTROL, x, y, z, 0, duration, 0.5, name, topicName);
}

void WaypointManager::addCircleFlight(double centerX, double centerY, double centerZ,
                     double radius, double duration, const std::string &name)
{
    waypoints.emplace_back(CIRCLE, centerX, centerY, centerZ, 0, duration, 0.5, name, "", radius);
}

void WaypointManager::addPoleSurround(double pole_x, double pole_y, double radius, double height, bool clockwise, const std::string &name)
{
    waypoints.emplace_back(POLE_SURROUND, pole_x, pole_y, height, clockwise, 0, 0.5, name, "", radius);
}

void WaypointManager::addHover(double duration, const std::string &name)
{
    waypoints.emplace_back(HOVER, 0, 0, 0, 0, duration, 0, name);
}

void WaypointManager::addWait(double duration, const std::string &name)
{
    waypoints.emplace_back(WAIT, 0, 0, 0, 0, duration, 0, name);
}

void WaypointManager::addSafeLanding(const std::string &name)
{
    waypoints.emplace_back(SAFE_LANDING, 0, 0, 0, 0, 0, 0.2, name);
}

void WaypointManager::addOffboardEnable(const std::string &name)
{
    waypoints.emplace_back(OFFBOARD_ENABLED, 0, 0, 0, 0, 0, 0, name);
}

void WaypointManager::reset()
{
    waypoints.clear();
    actionInProgress = false;
    initialized = false;
}

void WaypointManager::finalize()
{
    if (!waypoints.empty())
    {
        currentWaypoint = waypoints.begin();
        initialized = true;
        ROS_INFO_THROTTLE(1.0,  "WaypointManager initialized with %lu waypoints", waypoints.size());

        int index = 0;
        for (auto it = waypoints.begin(); it != waypoints.end(); ++it)
        {
            ROS_INFO_THROTTLE(1.0,  "Waypoint %d: %s (Type: %d)", index++, it->stepName.c_str(), it->actionType);
        }
    }
    else
    {
        ROS_WARN("No waypoints to initialize!");
    }
}

bool WaypointManager::isCurrentActionComplete(move_map &move1, ros::Publisher &local_vel_pub, ros::Rate &rate)
{
    if (!actionInProgress)
    {
        startCurrentAction();
        return false;
    }

    Waypoint current = getCurrentWaypoint();

    if (current.stepName != lastStepName) actionStartTime = ros::Time::now();
    lastStepName = current.stepName;

    ros::Duration elapsed = ros::Time::now() - actionStartTime;

    switch (current.actionType)
    {
    case OFFBOARD_ENABLED:
    {
        return move1.performOffboardEnable(rate);
    }
    
    case MOVE_TO_POINT:
    {
        geometry_msgs::PoseStamped target;
        target.pose.position.x = current.x;
        target.pose.position.y = current.y;
        target.pose.position.z = current.z;
        return move1.fly_to_target(target, local_pose_pub, rate, current.yaw) == 1;
    }

    case ROTATE:
    {
        return move1.performRotation(current.yaw, rate);
    }

    case TOPIC_CONTROL:
    {
        if (elapsed.toSec() >= current.duration)
        {
            ROS_INFO_THROTTLE(1.0,  "Topic control duration reached (%.1fs), ending control", current.duration);
            return true;
        }
        
        if (currentTopicName != current.topicName)
        {
            ROS_WARN("Topic changed from '%s' to '%s', re-subscribing", 
                     currentTopicName.c_str(), current.topicName.c_str());
            resetTopicControl();
            if (nh_ptr)
            {
                setupTopicSubscription(*nh_ptr, current.topicName);
            }
        }
        
        if (!hasTopicControl)
        {
            static ros::Time last_no_control_time;
            static bool no_control_initialized = false;
            
            if (!no_control_initialized) {
                last_no_control_time = ros::Time::now();
                no_control_initialized = true;
            }
            
            move1.hold(local_pose_pub, rate);
            
            ROS_WARN_THROTTLE(5.0, "No topic control message received from %s, using hold to maintain current position.", 
                             current.topicName.c_str());
        }
        else
        {
            double vx = std::max(-1.0, std::min(1.0, topicControlMsg.pose.position.x));
            double vy = std::max(-1.0, std::min(1.0, topicControlMsg.pose.position.y));
            double vz = std::max(-1.0, std::min(1.0, topicControlMsg.pose.position.z));
            double yaw = 0;
            move1.pose_velocity_ctrl(vx, vy, vz, yaw, rate);
            
            ROS_INFO_THROTTLE(2.0, "Topic control active from %s: velocity [%.2f, %.2f, %.2f]",
                             current.topicName.c_str(), vx, vy, vz);
        }
        return false;
    }
    
    case SUPER_CONTROL:
    {
        if (elapsed.toSec() >= current.duration)
        {
            ROS_INFO_THROTTLE(1.0,  "Topic control duration reached (%.1fs), ending control", current.duration);
            return true;
        }

        superControlPub = nh_ptr->advertise<geometry_msgs::PoseStamped>("/planning/click_goal", 10);
        static ros::Time super_last_time;
        static bool super_time_initialized = false;
        if (!super_time_initialized)
        {
            super_last_time = ros::Time::now();
            super_time_initialized = true;
        }

        if ((ros::Time::now() - super_last_time).toSec() >= 1.0)
        {
            geometry_msgs::PoseStamped target;
            // target.pose.position.x = -current.y;
            // target.pose.position.y = current.x; //莫说青山多阻碍
            target.pose.position.x = current.x;
            target.pose.position.y = current.y;
            target.pose.position.z = current.z;
            target.header.frame_id = "world";
            target.header.stamp = ros::Time::now();
            target.pose.orientation.w = 1.0;  // w为1的时候再规划，w为0的时候（收不到时）不规划
            superControlPub.publish(target);
            super_time_initialized = false;
        }
 
        if (currentTopicName != current.topicName)
        {
            ROS_WARN("Topic changed from '%s' to '%s', re-subscribing", 
                     currentTopicName.c_str(), current.topicName.c_str());
            resetTopicControl();
            if (nh_ptr)
            {
                setupTopicSubscription(*nh_ptr, current.topicName);
            }
        }
        
        if (!hasTopicControl)
        {
            static ros::Time last_no_control_time;
            static bool no_control_initialized = false;
            
            if (!no_control_initialized) {
                last_no_control_time = ros::Time::now();
                no_control_initialized = true;
            }
            
            move1.hold(local_pose_pub, rate);
            ROS_WARN_THROTTLE(5.0, "No topic control message received from %s, using hold to maintain current position.", 
                             current.topicName.c_str());
        }
        else
        {
            geometry_msgs::PoseStamped targetPose;
            targetPose.pose.position.x = topicControlMsg.pose.position.x;
            targetPose.pose.position.y = topicControlMsg.pose.position.y;
            targetPose.pose.position.z = topicControlMsg.pose.position.z;
            move1.fly_to_target(targetPose, local_pose_pub, rate, 3);
            ROS_INFO_THROTTLE(2.0, "Topic control active from %s: position [%.2f, %.2f, %.2f]",
                             current.topicName.c_str(), targetPose.pose.position.x, targetPose.pose.position.y, targetPose.pose.position.z);
        }
        // return move1.isReachedTarget(current.x, current.y, current.z, 0.2);
        return move1.isReachedTarget(topicControlMsg.pose.position.x, topicControlMsg.pose.position.y, topicControlMsg.pose.position.z, 0.2);
    }

    case CIRCLE:
    {
        if (elapsed.toSec() >= current.duration)
        {
            return true;
        }
        return move1.performCircleFlight(current.x, current.y, current.z, current.radius, elapsed.toSec(), rate) == 1;
    }

    case POLE_SURROUND: //yaw0为顺时针
    {
        return move1.performPoleSurrounding(current.x, current.y, current.radius, current.z, current.yaw) == 1;
    }

    case HOVER:
    case WAIT:
    {
        move1.hold(local_pose_pub, rate);
        return elapsed.toSec() >= current.duration;
    }

    case SAFE_LANDING:
    {
        return move1.performSafeLanding(current.speed, rate);
    }

    default:
        return true;
    }
};
